/*
Written by: Sam Lee
November 03, 2009
*/

#include <p18f458.h>
#include <timers.h>
#include <usart.h>
#include <delays.h>

#pragma config OSC = HS   
#pragma config WDT = OFF
#pragma config LVP = OFF 
#pragma config DEBUG = OFF 

int servo0 = 0x00; //Right Servo //0xF63B; // Servo 0 (Trigger Interrupted after 1mS)
int servo1 = 0x00; //Left Servo //0xF077; // Servo 1 (Trigger Interrupted after 1.6mS)
int servo2 = 0x00; // Servo 2
int servo3; // Servo 3
int servo4; // Servo 4
int servo5 = 0xF077; // Servo 5

int count = 0;

unsigned char c;

void InterruptHandlerHigh (void);

void main(void)
{
  //Set I/O ports
  TRISA = 0x00;
  TRISB = 0x00;
  TRISC = 0x00;	
  OpenUSART(USART_TX_INT_OFF & 
			USART_RX_INT_OFF & 
			USART_ASYNCH_MODE & 
			USART_EIGHT_BIT &   
			USART_CONT_RX & 
			USART_BRGH_LOW, 32); //9.6K

  //Clear Output Ports
  PORTA = 0x00;
  PORTB = 0x00;

  RCON = 0b000000000; 
  INTCON = 0b10100000;

  OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2 );
  OpenTimer1( TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_INT & T1_PS_1_2 & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF );

  WriteTimer0( 0x3CAF );	//Trigger Interrupted after 20mS
  WriteTimer1( 0xF63B );  //This is just a small initial delay chosen at random. (Trigger Interrupted after 1mS)
 
  while(1){

    while(!DataRdyUSART());
    c = getcUSART(); //c = ReadUSART();

	//servo0 = 0xFB1D;// Servo 0 -  (Trigger Interrupted after 0.5mS
	//servo0 = 0xF63B;// Servo 0 - (Trigger Interrupted after 1mS)
	//servo0 = 0xEC77;// Servo 0 - (Trigger Interrupted after 2mS)

	if (c=='8') //forward 
	{
	  servo0 = 0xFB1D;// Servo 0 -  
      servo1 = 0xEC77;// Servo 1 - 
      servo2 = 0xF190;
	  Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#8");
    }

    if (c=='2') //back
    {
   	  servo0 = 0xEC77;// Servo 0 - 
	  servo1 = 0xFB1D;// Servo 1 - 
      //servo2 = 0xFB1D;
      Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#2");
    }
    if (c=='5')  //stop
    {
	  servo0 = 0xF190;// Servo 0 - 
	  servo1 = 0xF190;// Servo 1 -
      //servo2 = 0xF63B;
	  Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#5");
    }
	if (c=='6')  //right 
	{
	  servo0 = 0xF190;// Servo 0 -
      servo1 = 0xEC77;// Servo 1 -
	  Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#6");
    }
    if (c=='4')  //left
    {
   	  servo0 = 0xFB1D;// Servo 0 -
	  servo1 = 0xF190;// Servo 1 -
      Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#4");
    }

    if (c=='9')  //back right 
    {
	  servo0 = 0xF190;// Servo 0 - 
	  servo1 = 0xFB1D;// Servo 1 -
	  Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#9");
    }
    if (c=='1')  //back left 
    {
	  servo0 = 0xEC77;// Servo 0 -
	  servo1 = 0xF190;// Servo 1 -
	  Delay10KTCYx(250);	// 1 Second Delay
	  Delay10KTCYx(250);
      putrsUSART("#1");
    }
  }
}

//INTERRUPT CONTROL
#pragma code InterruptVectorHigh = 0x08 //0x08                //interrupt pointer address (0x18 low priority)
void InterruptVectorHigh (void)
{
        _asm        //assembly code starts
        goto InterruptHandlerHigh                //interrupt control
        _endasm //assembly code ends
}
#pragma code
#pragma interrupt InterruptHandlerHigh        //enf.

void InterruptHandlerHigh()        // declaration of InterruptHandler
{//this gets ran when ever the timers flop over from FFFF->0000
        if(INTCONbits.TMR0IF)                        //check if TMR0 interrupt flag is set
        {
				WriteTimer0( 0x3CAF );
				WriteTimer1( 0xFC77 );
				count = 0;
			    INTCONbits.TMR0IF = 0;                //clear TMR0 flag               
        }
        if(PIR1bits.TMR1IF == 1 && PIE1bits.TMR1IE == 1)        //if set controls the first servo
        {
		count++;	
				switch(count){		
			case 1:     PORTB = 0x01; // First Stage
					    WriteTimer1( servo0 );
						break;
			case 2:		PORTB = 0x02; // Servo 1
						WriteTimer1( servo1 );
						break;
			case 3:		PORTB = 0x04;
						//PORTC = 0x02; // Swivel/Rotate
						WriteTimer1( servo2 );
						break;
			case 4:		PORTB = 0x00;
						//WriteTimer1( servo3 ); 
						break;
			case 5:		
						//WriteTimer1( servo4 ); 
						break;
			case 6:		
						//WriteTimer1( 0 );
						break;
				}

                PIR1bits.TMR1IF = 0;                        //clear Timer1 flag
                PIE1bits.TMR1IE = 1;                        //clear Timer1 enable flag set to zero
        }

    INTCONbits.GIE = 1;                                //re-enable all interrupts
}

